/*
 * 用以摇杆控制四个电机
 */
#include <MultiStepper.h>
#include <AccelStepper.h>
#define Mode_DRIVER 1
#define forward_step 9 
#define forward_dir 8 
#define vertical_step 7 
#define vertical_dir 6   
#define rotate_step 5  
#define rotate_dir 4 
#define clutch_step 3 
#define clutch_dir 2 
AccelStepper vertical(Mode_DRIVER, vertical_step, vertical_dir);
AccelStepper rotate(Mode_DRIVER, rotate_step, rotate_dir);
AccelStepper clutch(Mode_DRIVER, clutch_step, clutch_dir);
AccelStepper forward(Mode_DRIVER, forward_step, forward_dir);

//char serialData;
int nx = 0,ny = 0,nc = 0,nv = 0; //方向变量
long sx=0,sy=0; // front和rotate速度大小变量
int subx = 400, suby = 6400,subc = 24,subv = 400; // 细分变量
int xmaxspeed, ymaxspeed,s; //最大速度变量
int cspeed,vspeed; 从，和v方向的匀速运动的速度大小
int xsignal,ysignal,csignal,vsignal; // 模拟输入变量
void readana(); //读取模拟信号
float  t = 0; // 控制读取模拟型号的频率的时间变量
void setup() {
  Serial.begin(9600);
  Serial.print(F("请输入旋转方向电机细分:"));
  while (!Serial.available()) {}
  subx = Serial.parseInt();
  Serial.println(subx);
  Serial.print(F("请输入前进方向电机细分:"));
  while (!Serial.available()) {}
  suby = Serial.parseInt();
  Serial.println(suby);
  Serial.print(F("请输入抓取方向电机细分:"));
  while (!Serial.available()) {}
  subc = Serial.parseInt();
  Serial.println(subc);
   Serial.print(F("请输入升降方向电机细分:"));
  while (!Serial.available()) {}
  subv = Serial.parseInt();
  Serial.println(subv);
  Serial.println(F("请输入旋转方向最大速度:"));
  while (!Serial.available()) {}
  xmaxspeed = Serial.parseInt();
  Serial.println(xmaxspeed);
  Serial.println(F("请输入前进方向最大速度:"));
  while (!Serial.available()) {}
  ymaxspeed = Serial.parseInt();
  Serial.println(ymaxspeed);
  Serial.println(F("请输入抓取方向速度:"));
  while (!Serial.available()) {}
  cspeed = Serial.parseInt();
  Serial.println(cspeed);
  Serial.println(F("请输入升降方向速度:"));
  while (!Serial.available()) {}
  vspeed = Serial.parseInt();
  Serial.println(vspeed);
  forward.setMaxSpeed((float)(ymaxspeed / 360 * suby));
  rotate.setMaxSpeed((float)(xmaxspeed / 360 * subx));
  clutch.setMaxSpeed((float)(cspeed / 360 * subc));
  vertical.setMaxSpeed((float)(vspeed / 360 * subv));
  Serial.print("前进方向最大速度:");
  Serial.println((ymaxspeed / 360 * suby));
  Serial.print("旋转方向最大速度:");
  Serial.println((xmaxspeed / 360 * subx));
}

// the loop function runs over and over again until power down or reset
void loop() {
//  if(Serial.available()){
//    serialData = Serial.read();
//    Serial.print(serialData);
//  }
  if(millis()-t>=100)
  {
      readana();
      t = millis();
      sx = map(s,0,1023,0,xmaxspeed);
      sy = map(s,0,1023,0,ymaxspeed);
     if (xsignal - 542 > 100)
        nx = 1;
     else if(xsignal - 542 < -100)
        nx = -1;
//        Serial.println("逆转");
     else
        nx = 0;
     if (ysignal-554 > 100)
        ny = 1;
     else if(ysignal-554<-100)
        ny = -1;
     else
        ny = 0;
     if (csignal - 467>100)
        nc = 1;
     else if(csignal -467<-100)
        nc = -1;
     else
        nc = 0;
     if (vsignal - 463>100)
        nv = 1;
     else if(vsignal -463<-100)
        nv = -1;
     else
        nv = 0;
     rotate.setSpeed((float)(nx * sx / 360* subx));
     forward.setSpeed((float)(ny * sy / 360 * suby));
     clutch.setSpeed((float)(cspeed *nc / 360 * subc));
     vertical.setSpeed((float)(vspeed * nv/ 360 * subv));
}
 forward.runSpeed();
 rotate.runSpeed();
 clutch.runSpeed();
 vertical.runSpeed();
}

void readana()
{
  xsignal = analogRead(A0);
  ysignal = analogRead(A1);
  csignal = analogRead(A4);
  vsignal = analogRead(A5); 
  s = analogRead(A2);
}
